#include "StdAfx.h"

#include "Trajectory.h"
#include "ApproachSearcher.h"

CTrajectory::CTrajectory(CEphemWrapper::Ptr pEphem, CAsteroidOrbitParam::Ptr pAsteroidOrbitParam, double dStep)
: m_pEphem(pEphem)
, m_dStep(dStep)
, m_Integrator(CIntergator::eEverhard)
{

	vector<eBodies> vecBodyesNum;
	vecBodyesNum.push_back(eEARTH);
	//vecBodyesNum.push_back(eMOON);

	m_pAppSearcher.reset( new CApproachSearcher(this, m_pEphem.impl(), vecBodyesNum, pAsteroidOrbitParam));
}

CTrajectory::~CTrajectory()
{

}

ListPtrApproachs CTrajectory::Move(double dStartTime, double dEndTime, SBody &body, TimePosTrace * pTrace)
{
	ListPtrApproachs listApproaches;
	if(pTrace)
	{
		TimePosTrace & trace = *pTrace;
		TrajTrace::Ptr pos(new TrajTrace(body));
		trace[dStartTime] = pos;
	}

	while(dStartTime != dEndTime)
	{
		double dCurStep = m_dStep;
		double dTime = dEndTime - dStartTime;

		if (fabs(dTime) < dCurStep)
			dCurStep = fabs(dTime);

		if( dTime < 0.0)
			dCurStep = -dCurStep;

		ListPtrApproachs curList = m_pAppSearcher->Check(dStartTime, body);
		listApproaches.insert(listApproaches.end(), curList.begin(), curList.end());

		m_Integrator.Move(dStartTime, dStartTime + dCurStep, body.x, body.v, this);

		dStartTime += dCurStep;
		if(pTrace)
		{
			TimePosTrace & trace = *pTrace;
			TrajTrace::Ptr pos( new TrajTrace(body) );
			trace[dStartTime] = pos;
		}
		//printf("%lf %s | %lf %lf\n", dStartTime, CEphemWrapper::StrDateFromJD(dStartTime).c_str(), body.x[0], body.x[1]);
	}
	return listApproaches;
}

bool CTrajectory::SimpleMove(double start, double end, SBody &body)
{
	return m_Integrator.Move(start, end, body.x, body.v, this);
}

bool CTrajectory::Force(int nv, double Time, double *x, double *f)
{
	if(nv != 3)
		throw CCustomException(_T("nv must be 3"));
	
	double time = Time;
	AllBodies bodies;
	m_pEphem->GetCoord(time, true, bodies, false);

	{
		for(int i = 0; i<nv; i++)
			f[i] = 0.0;
	
		for(int body = 0; body < sizeof(bodies.bodies)/sizeof(bodies.bodies[0]); ++body)
		{
			double d[3];
			double rr = 0.0;
			const SBody & CurBody = bodies.bodies[body];
			for(int i = 0; i < nv; i++)
			{
				d[i] = CurBody.x[i] - x[i];
				rr += d[i] * d[i];
			}

			double r = sqrt(rr);
			double rrr = r * rr;
			double K = CurBody.GM / rrr;
			for(int i = 0; i < nv; i++)
				f[i] +=  K * d[i];
		}
	}
	
	return true;
}	

